package com.sensors;

import android.util.Log;

import java.util.Arrays;

/**
 * Created by tjpu on 2016/10/12.
 */
public class Gyroscope{
    static String thisLogTag = "Gyroscope";

    public double receivePeriod = 0.1;
    public static double CONVERT_TO_METRIC = 1.0/1024.0;
    public static double PERIOD = 0.1;

    public double[] angle = {0.0, 0.0, 0.0};

    long lastTime = System.currentTimeMillis();
    public short[] origin = {0, 0, 0};
    double[] metricUnit = {0.0, 0.0, 0.0};

    public int notReceive = 0;
    public boolean isNotReceive = false;

    public void putOrigin(short[] gyro) {
       for(int i=0; i<origin.length && i < gyro.length; i++) {
           origin[i] = gyro[i];
       }

        convert();
        judge();
        filter();
        integral();

        notReceive = 0;
        isNotReceive = false;
        receivePeriod = (System.currentTimeMillis() - lastTime) /  1000.0;
        lastTime = System.currentTimeMillis();
        Log.i(thisLogTag,"receive period: "+receivePeriod);
    }

    public double[] getMetric() {
        double[] returnData = new double[metricUnit.length];
        System.arraycopy(metricUnit,0,returnData,0,returnData.length);
        return returnData;
    }

    public double[] getAngle() {
        double[] returnData = new double[angle.length];
        System.arraycopy(angle,0,returnData,0,returnData.length);
        return returnData;
    }

    void convert() {
        for(int i=0; i<origin.length; i++) {
            metricUnit[i] = origin[i] * CONVERT_TO_METRIC;
        }
    }

    void judge() {

    }

    void integral() {
        for (int i=0; i < metricUnit.length; i++) {
            angle[i] += metricUnit[i] * PERIOD;
        }
        Log.i(thisLogTag, "angle: "+Arrays.toString(angle));
    }

    void filter() {

    }

    public void checkReceive() {
        notReceive++;
        if (notReceive > 3 && !isNotReceive) {
            isNotReceive = true;
            Log.e(thisLogTag,"can't receive data");
        }
    }
}
